#include <iostream>
#include <pcl/ModelCoefficients.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/filters/extract_indices.h>

int main (int argc, char** argv)
{
    // std::cout << "Hello, it's my first pcl cmake project!" << std::endl;

    // 第1步： 处理参数
    double threshold;
    std::string infile = argv[1];                // pcd文件名 (带扩展名：.pcd)
    std::istringstream (argv[2]) >> threshold;   // threshold

    // 第2步： 从pcd文件读入点云
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>), cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>), cloud_projected (new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PCDReader reader;
    reader.read (infile, *cloud);

    // 第3步： 使用RANSAC方法分割出平面
    pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);  // 用于存放分割平面方程的4个参数 (ax+by+cz+d=0)
    pcl::PointIndices::Ptr inliers (new pcl::PointIndices);                 // 用于存放分割出来的平面点在点云cloud中的下标
    pcl::SACSegmentation<pcl::PointXYZ> seg;    // 创建分割对象
    seg.setOptimizeCoefficients (true);         // 可选参数，其他均为必选参数
    seg.setModelType (pcl::SACMODEL_PLANE);
    seg.setMethodType (pcl::SAC_RANSAC);
    seg.setDistanceThreshold (threshold);
    seg.setInputCloud (cloud);
    seg.segment (*inliers, *coefficients);

    // 第4步： 保存平面部分和其他部分到pcd文件
    // 平面部分
    pcl::ExtractIndices<pcl::PointXYZ> extract;
    extract.setInputCloud (cloud);
    extract.setIndices (inliers);
    extract.filter (*cloud_filtered);
    std::cerr << "inliers cloud after filtering: " << std::endl;
    std::cerr << *cloud_filtered << std::endl;  // 这里有几行点云信息输出
    pcl::PCDWriter writer;
    writer.write<pcl::PointXYZ> ("res_inliers.pcd", *cloud_filtered, false);
    // 其他部分
    extract.setNegative (true);
    extract.filter (*cloud_filtered);
    std::cerr << "others cloud after filtering: " << std::endl;
    std::cerr << *cloud_filtered << std::endl;  // 这里有几行点云信息输出
    writer.write<pcl::PointXYZ> ("res_others.pcd", *cloud_filtered, false);

    return 0;
}